
#include "capabilities.h"
#include "ldac_dec_priv_lib.h"
#include "ldac_buffer_control.h"
#include "../a2dp_decode/a2dp_common_decode.h"
#include "patch/patch.h"

/***************************************************************************
Private Function Declarations
*/
#ifdef ENABLE_BUFFER_CONTROL
void bc_mem_destroy(LDAC_BUFF_CTRL_OP *hBufCtrl) {

    if(hBufCtrl->cb_frame_sts != NULL )
        cbuffer_destroy(hBufCtrl->cb_frame_sts);
    
}

int bc_mem_create(LDAC_BUFF_CTRL_OP *hBufCtrl) {
    hBufCtrl->cb_frame_sts = NULL;
    /*frame status buffer of 100frames */
    hBufCtrl->cb_frame_sts = cbuffer_create_with_malloc(LDAC_BUFFER_CONT_STS_BUF_SIZE, BUF_DESC_LOCAL_MMU_BUFFER);
    if (hBufCtrl->cb_frame_sts == NULL)
    {
        return FALSE;
    }
    return TRUE;
}

void bc_init(LDAC_BUFF_CTRL_OP *hBufCtrl) {
    hBufCtrl->bc = bc_new(DECODER_NUM_FRAMES_STARTS_DECODE);
    hBufCtrl->bc->skip_num=0xFF;/* reset skip num flg */
    hBufCtrl->bc->skip_flg = 0;
    hBufCtrl->bc->skip_cnt=0;
    hBufCtrl->bc->skip_itr_error=0;
    hBufCtrl->bc->copy_cnt=0;
    hBufCtrl->bc->copy_itr_error=0;
}


#endif


void reset_cbuffers( tCbuffer *cb )
{
    // reset cbuffer read/write pointer
    unsigned int *addr = (unsigned int*)cb->base_addr;
    cbuffer_set_read_address_ex(cb, addr, 0u);
    cbuffer_set_write_address_ex(cb, addr, 0u);
}


static void bc_swap_endian_int32( unsigned char *p ){
    unsigned char uc0, uc1, uc2, uc3;
    uc0 = p[0];  uc1 = p[1];  uc2 = p[2];  uc3 = p[3];
    p[0] = uc3;  p[1] = uc2;  p[2] = uc1;  p[3] = uc0;
}

static unsigned bc_read_ldac_frame_header( unsigned char *p_stream,
                                           int *sampling_rate_index, int *channel_config_index,
                                           int *frame_length_index, int *frame_status )
{
    /*---------------------------------------------------*/
    /* | p_stream[0]   | p_stream[1]   | p_stream[2]   | */
    /* |0 1 2 3 4 5 6 7|0 1 2 3 4 5 6 7|0 1 2 3 4 5 6 7| */
    /* | A             |  B  | C |  D              | E | */
    /*---------------------------------------------------*/
    /*    A: 8bit : syncword                             */
    /*    B: 3bit : sampling_rate_index                  */
    /*    C: 2bit : channel_config_index                 */
    /*    D: 9bit : frame_length_index                   */
    /*    E: 2bit : frame_status                         */
    /*---------------------------------------------------*/
    if( p_stream[0] != 0xAA ){
        L2_DBG_MSG3("LDAC bc_read_ldac_frame_header : worng syncword! [%x|%x|%x]", p_stream[0],p_stream[1],p_stream[2]);
        return CODEC_ERROR;
    }
    *sampling_rate_index = (p_stream[1] >> 5) & 0x7;
    *channel_config_index =  (p_stream[1] >> 3) & 0x3;
    *frame_length_index = p_stream[1] << 8 | p_stream[2];
    *frame_length_index >>= 2;
    *frame_length_index &= 0x000001FF;
    
    *frame_status = p_stream[2];
    *frame_status &= 0x00000003;
    return CODEC_SUCCESS;
}

static unsigned bc_get_ldac_frame_size( tCbuffer *cb_buf, tCbuffer *cb_work, int *p_read_bytes, int *pframe_size, int *pframe_status )
{
    unsigned int *addr, offset;
    unsigned char *p_stream, a_head[3];
    int sampling_rate_index, channel_config_index, frame_length_index;
    *pframe_size = 0;
    *pframe_status = 0;
    /* read only config data (4byte aligned) */
    reset_cbuffers(cb_work); // reset
    *p_read_bytes = cbuffer_copy_ex(cb_work, cb_buf, 4);
    
    if(*p_read_bytes < 4){
          L2_DBG_MSG1("bc_get_ldac_frame_size() : Not Enough Input DATA. n_read_bytes=%d",*p_read_bytes);
        return CODEC_NOT_ENOUGH_INPUT_DATA;
    }
    addr = cbuffer_get_read_address_ex(cb_work, &offset); // offset may be allways 0
    p_stream = (unsigned char*)(addr + offset);
    a_head[0] = p_stream[3];
    a_head[1] = p_stream[2];
    a_head[2] = p_stream[1];
    if( CODEC_ERROR == bc_read_ldac_frame_header(a_head, &sampling_rate_index, &channel_config_index,
                                                   &frame_length_index, pframe_status)){
        return CODEC_ERROR;
    }
    *pframe_size = frame_length_index + 4;

    return CODEC_SUCCESS;
}

#ifdef ENABLE_BUFFER_CONTROL
static void proceed_write_buff(tCbuffer *cb_ring,int data){
    unsigned int *addr, offset;
    unsigned buff_size;

    addr = cbuffer_get_write_address_ex(cb_ring,&offset);
    *addr = data;
    buff_size = cbuffer_get_size_in_words(cb_ring);
    addr++;
    if(addr == (unsigned int*)cb_ring->base_addr+buff_size){
        addr = (unsigned int*)cb_ring->base_addr;
    }    
    cbuffer_set_write_address_ex(cb_ring, addr, offset);
}

static void proceed_read_buff(tCbuffer *cb_ring,int* data){
    unsigned int *addr, offset;
    unsigned buff_size;
    
    addr = cbuffer_get_read_address_ex(cb_ring,&offset);
    *data=*addr;
    buff_size = cbuffer_get_size_in_words(cb_ring);
    addr++;
    if(addr == (unsigned int*)cb_ring->base_addr+buff_size){
        addr = (unsigned int*)cb_ring->base_addr;
    }
    cbuffer_set_read_address_ex(cb_ring, addr, offset);    
}
#endif

static void rxbuf_op_sync_clone( LDAC_DEC_RX_BUF *hRxBuf )
{
    unsigned *addr, offset;
    addr = cbuffer_get_read_address_ex(hRxBuf->main, &offset);
    cbuffer_set_read_address_ex(hRxBuf->clone, addr, offset);
    addr = cbuffer_get_write_address_ex(hRxBuf->main, &offset);
    cbuffer_set_write_address_ex(hRxBuf->clone, addr, offset);
}

static void rxbuf_op_discard_data( LDAC_DEC_RX_BUF *hRxBuf, int bytes )
{
    /* advance read pointer and sync with clone */
    cbuffer_advance_read_ptr_ex(hRxBuf->main, bytes);
    rxbuf_op_sync_clone(hRxBuf);
}

static unsigned rxbuf_op_discard_frame_data( LDAC_DEC_RX_BUF *hRxBuf, tCbuffer *cb_work)
{
    int n_read_bytes, size, data_num;
    int sts;
    unsigned mode = bc_get_ldac_frame_size( hRxBuf->clone, cb_work, &n_read_bytes, &size, &sts);
    if( mode != CODEC_SUCCESS ) {
        rxbuf_op_sync_clone(hRxBuf);
        return mode;
    }

    data_num = cbuffer_calc_amount_data_ex(hRxBuf->main);
    if( data_num < size ) {
        rxbuf_op_sync_clone(hRxBuf);
        return CODEC_NOT_ENOUGH_INPUT_DATA;
    }

    /* discard stream data */
    rxbuf_op_discard_data(hRxBuf, size);
    if( (hRxBuf->nFrame--) < 0 ){ hRxBuf->nFrame = 0; }
    return CODEC_SUCCESS;
}


static int check_fs(int fs_id,int sampling_freq)
{
    if(fs_id == 0x03){
        if(sampling_freq==96000){
            //fs OK
            return 1;
        }else{
            return 0;
        }
    }else if(fs_id == 0x02){
        if(sampling_freq==88200){
            //fs OK
            return 1;
        }else{
            return 0;
        }
    }else if(fs_id == 0x01){
        if(sampling_freq==48000){
            //fs OK
            return 1;
        }else{
            return 0;
        }
    }else if(fs_id == 0x00){
        if(sampling_freq==44100){
            //fs OK
            return 1;
        }else{
            return 0;
        }
    }else{
        return 0;
    }
}

static int check_ch(int ch_id,int channel_mode)
{
    if(ch_id == 0x00){//mono
        if(channel_mode==0x04){
            //ch OK
            return 1;
        }else{
            return 0;
        }
    }else if(ch_id == 0x01){//dual
        if(channel_mode==0x02){
            //ch OK
            return 1;
        }else{
            return 0;
        }
    }else if(ch_id == 0x02){//stereo
        if(channel_mode==0x01){
            //ch OK
            return 1;
        }else{
            return 0;
        }
    }else{
        return 0;
    }
}

static unsigned search_next_frame_header( LDAC_DEC_RX_BUF *hRxBuf, tCbuffer *cb_out,int frame_size,int sampling_freq,int channel_mode )
{
    int p_read_bytes,i;
    unsigned char fs_id,ch_id;
    unsigned *addr,*addr1, offset;
    unsigned char *p_stream, a_head[4];
    tCbuffer *cb_buf,*cb_work;
    int flag_aa;
    int total_skip_size;
    
    total_skip_size=0;
    cb_buf = hRxBuf->clone;
    cb_work = cb_out;
    flag_aa=0;
    
    for(;;){
        reset_cbuffers(cb_work); // reset
        addr1 = cbuffer_get_read_address_ex(cb_buf, &offset); // offset may be allways 0
        p_read_bytes = cbuffer_copy_ex(cb_work, cb_buf, 4);
        if(p_read_bytes < 4){
            L2_DBG_MSG1("search_next_frame_header() : Not Enough Input DATA. n_read_bytes=%d",p_read_bytes);
            return CODEC_NOT_ENOUGH_INPUT_DATA;
        }
        addr = cbuffer_get_read_address_ex(cb_work, &offset); // offset may be allways 0
        p_stream = (unsigned char*)(addr + offset);
        a_head[0] = p_stream[3];
        a_head[1] = p_stream[2];
        a_head[2] = p_stream[1];
        a_head[3] = p_stream[0];
        for(i=0;i<4;i++){
            if(!flag_aa){
                if(a_head[i] == 0xAA){
                    if(i < 3 ){
                        //fs check
                        fs_id = (a_head[i+1] & 0xE0)>>5;
                        if(check_fs(fs_id,sampling_freq)){
                            ch_id = (a_head[i+1] & 0x18)>>3;
                            if(check_ch(ch_id,channel_mode)){
                                goto FRAME_FOUND;
                            }
                        }
                    }else{
                        flag_aa=1;
                    }
                }
            }else{
                //found 0xAA previous buffer.
                //fs check
                fs_id = (a_head[i] & 0xE0)>>5;
                if(check_fs(fs_id,sampling_freq)){
                    ch_id = (a_head[i] & 0x18)>>3;
                    if(check_ch(ch_id,channel_mode)){
                        goto FRAME_FOUND;
                    }
                }
                flag_aa=0;
            }
        }
        cbuffer_set_read_address_ex(cb_buf,addr1+1,offset);
        total_skip_size+=4;
        if((total_skip_size > frame_size)&&(frame_size)){
            hRxBuf->nFrame--;
        }
    }
FRAME_FOUND:    
    //found sync data
    if(!flag_aa){
        offset +=i; 
        cbuffer_set_read_address_ex(cb_buf,addr1,offset);
    }else{
        offset +=3; 
        cbuffer_set_read_address_ex(cb_buf,addr1-1,offset);
    }
    return CODEC_SUCCESS;
}

static unsigned rxbuf_op_peek_frame_data( LDAC_DEC_RX_BUF *hRxBuf, tCbuffer *cb_out,
                                          int *n_read_bytes, uint32 *status)
{
    int size, ii;
    int sts;
    unsigned *addr, offset;
    unsigned char *p_stream;
    unsigned mode = bc_get_ldac_frame_size( hRxBuf->clone, cb_out, n_read_bytes, &size, &sts);
    if( mode != CODEC_SUCCESS ) {
        return mode;
    }

    /* frame status */
    *status = sts;

    /* read stream data */
    *n_read_bytes += cbuffer_copy_ex(cb_out, hRxBuf->clone, (size-*n_read_bytes));
    if(*n_read_bytes != size ){
        return CODEC_NOT_ENOUGH_INPUT_DATA;
    }
    addr = cbuffer_get_read_address_ex(cb_out, &offset);
    p_stream = (unsigned char*)(addr + offset);
    /* swap endian */
    /* [ii+n] can be get bigger than n_read_bytes, so please make sure that the size of a_tream[] is big enough. */
    for (ii = 0; ii<*n_read_bytes; ii+=4) {
        bc_swap_endian_int32( &p_stream[ii] );
    }
    return CODEC_SUCCESS;
}

static void rxbuf_op_pop_frame_data( LDAC_DEC_RX_BUF *hRxBuf, int frame_bytes )
{
    rxbuf_op_discard_data(hRxBuf, frame_bytes);
    if( (hRxBuf->nFrame--) < 0 ){ hRxBuf->nFrame = 0; }
}

int ldac_decoder_stores_rx_data(tCbuffer *in_buffer, LDAC_DEC_PARAMS *pDecParams, int *nFrmRegistered )
{
    int nFrmRx, rx_octets, read_octets, space_octets, remaining_octets, payload_header;
    int n_read_bytes, frame_size, frame_status;
    unsigned int *addr, offset, nsft;
    LDAC_DEC_RX_BUF *hRxBuf = &pDecParams->RxBuf;
    tCbuffer *cb_carried = hRxBuf->carried;
    tCbuffer *cb_main = hRxBuf->main;
    tCbuffer *cb_clone = hRxBuf->clone;
    tCbuffer *cb_work = pDecParams->cb_stream;
#ifdef ENABLE_BUFFER_CONTROL
    tCbuffer *cb_frame_sts = pDecParams->hBufCtrl.cb_frame_sts;
#endif

    *nFrmRegistered = 0;
    read_octets = 0;

    // Get input data amount [byte]
    rx_octets =  cbuffer_calc_amount_data_ex(in_buffer);
    if( rx_octets == 0 ){
        
        goto CB_PUSH_RXDATA_RETURN;
    }


    // Get Rx buffer space
    space_octets = cbuffer_calc_amount_space_ex(cb_main);
    while( rx_octets > space_octets ){
        // ToDo: must send message to a2dp source to send flow control earlier.
        // tentative implementation
        rxbuf_op_discard_frame_data(hRxBuf, cb_work);
        space_octets = cbuffer_calc_amount_space_ex(cb_main);
    }

    while(hRxBuf->remaining_FrmNum > 0) {
        // calc remaing_octets and frame_size in each case
        if(hRxBuf->carried_Octets == 0){ // frame aligned. 
            if( bc_get_ldac_frame_size(in_buffer, cb_carried, &n_read_bytes, &frame_size, &frame_status) != CODEC_SUCCESS ){
                L2_DBG_MSG2("LDAC cb_push_RxData: error!! AAA::: rx_octets:%u, read_octets:%u", rx_octets, read_octets);
                goto SKIP_REGISTERING;
            }
#ifdef ENABLE_BUFFER_CONTROL
            /*proceeding frame sts buffer WP*/
            proceed_write_buff(cb_frame_sts,frame_status);
#endif
            remaining_octets = n_read_bytes;
            read_octets += n_read_bytes;
        }
        else if(hRxBuf->carried_Octets == -1) {
            unsigned char *p_stream, a_head[3];
            int sampling_rate_index, channel_config_index, frame_length_index;
            
            remaining_octets = cbuffer_calc_amount_data_ex(cb_carried);
            read_octets += cbuffer_copy_ex(cb_carried, in_buffer, (4-remaining_octets));
            remaining_octets = 4;
            addr = cbuffer_get_read_address_ex(cb_carried, &offset); // offset may be allways 0
            p_stream = (unsigned char*)(addr + offset);
            //bc_swap_endian_int32(p_stream);
            a_head[0] = p_stream[3];
            a_head[1] = p_stream[2];
            a_head[2] = p_stream[1];
            if( CODEC_ERROR == bc_read_ldac_frame_header(a_head, &sampling_rate_index,
                                                         &channel_config_index, &frame_length_index,
                                                         &frame_status)){
                L2_DBG_MSG2("LDAC cb_push_RxData: error!! BBB::: rx_octets:%u, read_octets:%u", rx_octets, read_octets);
                goto SKIP_REGISTERING;
            }
            frame_size = frame_length_index + 4;
        }
        else/* if(hRxBuf->carried_Octets > 0)*/{ // frame fragmented.
            if((rx_octets-read_octets) >= hRxBuf->carried_Octets){
                remaining_octets = cbuffer_calc_amount_data_ex(cb_carried);
                frame_size = remaining_octets + hRxBuf->carried_Octets;
                hRxBuf->carried_Octets = 0;
            }
            else{ // not enough yet
                read_octets += cbuffer_copy_ex(cb_carried, in_buffer, (rx_octets - read_octets));
                hRxBuf->carried_Octets = hRxBuf->carried_Octets - (rx_octets - read_octets);
                goto NOT_ENOUGH_DATA;
            }
        }

        if( (rx_octets - read_octets - (frame_size-remaining_octets)) < 0 ) {
            hRxBuf->carried_Octets = -(rx_octets - read_octets - (frame_size-remaining_octets));
            read_octets += cbuffer_copy_ex(cb_carried, in_buffer, (rx_octets - read_octets));
            goto NOT_ENOUGH_DATA;
        }
        cbuffer_copy_ex(cb_main, cb_carried, remaining_octets);
        read_octets += cbuffer_copy_ex(cb_main, in_buffer, (frame_size-remaining_octets));
        hRxBuf->remaining_FrmNum = hRxBuf->remaining_FrmNum - 1;
        hRxBuf->carried_Octets = 0;
        (*nFrmRegistered)++;
        reset_cbuffers(cb_carried); // reset
    }

    while((rx_octets - read_octets) > 0) {
        //
        // Read the payload header and get the LDAC frame number,
        // and then copy All payload data into cb_main by frame.
        reset_cbuffers(cb_work);
        read_octets += cbuffer_copy_ex(cb_work, in_buffer, 1);
        addr = cbuffer_get_read_address_ex(cb_work, &offset);
        payload_header = (int)*addr;
        nsft = 8 * (3 - offset); // offset must be 0, 1, 2 or 3.
        nFrmRx = (payload_header>>nsft) & 0xF;
        

        while(nFrmRx > 0) {
            
            if((rx_octets - read_octets) < 4){
                hRxBuf->remaining_FrmNum = nFrmRx;
                if(rx_octets > read_octets){
                    hRxBuf->carried_Octets = -1;
                    reset_cbuffers(cb_carried); // reset
                    read_octets += cbuffer_copy_ex(cb_carried, in_buffer, (rx_octets - read_octets));
                }
                goto NOT_ENOUGH_DATA;
            }
            // get frame size & copy first 4 byte
            if( bc_get_ldac_frame_size(in_buffer, cb_carried, &n_read_bytes, &frame_size, &frame_status ) != CODEC_SUCCESS ){
                L2_DBG_MSG2("LDAC cb_push_RxData: error!! not ldac frame header. ::: rx_octets:%u, read_octets:%u", rx_octets, read_octets);
                goto SKIP_REGISTERING;
            }
#ifdef ENABLE_BUFFER_CONTROL
            /*proceeding frame sts buffer WP*/
            proceed_write_buff(cb_frame_sts,frame_status);
#endif
            read_octets += n_read_bytes;
            if( (rx_octets - read_octets - (frame_size - n_read_bytes)) < 0 ) {
                hRxBuf->remaining_FrmNum = nFrmRx;
                hRxBuf->carried_Octets = -(rx_octets - read_octets - (frame_size - n_read_bytes));
                if(rx_octets > read_octets){
                    read_octets += cbuffer_copy_ex(cb_carried, in_buffer, (rx_octets - read_octets));
                }
                goto NOT_ENOUGH_DATA;
            }
            // Copy rest of frame data
            cbuffer_copy_ex(cb_main, cb_carried, n_read_bytes);
            read_octets += cbuffer_copy_ex(cb_main, in_buffer, (frame_size-n_read_bytes));
            nFrmRx--;
            (*nFrmRegistered)++;
            
        }
    }
NOT_ENOUGH_DATA:

#ifdef INSTALL_METADATA
    //  discard the metadata
    {
        unsigned b4idx, afteridx;
        int meta_len = 0;
        metadata_tag *mtag;
        mtag = buff_metadata_get_head(in_buffer);
        meta_len += mtag->length;
        mtag = buff_metadata_remove(in_buffer, read_octets/*mtag->length*/, &b4idx, &afteridx);
        buff_metadata_tag_list_delete(mtag);
        NOT_USED(meta_len);
    }
#endif
    addr = cbuffer_get_write_address_ex(cb_main, &offset);
    cbuffer_set_write_address_ex(cb_clone, addr, offset);

  CB_PUSH_RXDATA_RETURN:

    hRxBuf->nFrame += *nFrmRegistered;
    return CODEC_SUCCESS;

SKIP_REGISTERING:
    // reset to wait for next frame alinged input data
    hRxBuf->remaining_FrmNum = 0;
    hRxBuf->carried_Octets = 0;
    reset_cbuffers(cb_carried);
    rxbuf_op_sync_clone(hRxBuf);
    rx_octets =  cbuffer_calc_amount_data_ex(in_buffer);
    cbuffer_advance_read_ptr_ex(in_buffer, rx_octets);
#ifdef INSTALL_METADATA
    //  discard the metadata
    {
        unsigned b4idx, afteridx;
        int meta_len = 0;
        metadata_tag *mtag;
        mtag = buff_metadata_get_head(in_buffer);
        meta_len += mtag->length;
        mtag = buff_metadata_remove(in_buffer, rx_octets/*mtag->length*/, &b4idx, &afteridx);
        buff_metadata_tag_list_delete(mtag);
        NOT_USED(meta_len);
    }
#endif
    return CODEC_ERROR;
}

#ifdef ENABLE_BUFFER_CONTROL

static unsigned cb_frame_data_chk_sts_iterate( LDAC_BUFF_CTRL_OP *hBufCtrl, LDAC_DEC_RX_BUF *hRxBuf,uint32 frm_num)
{
    int sts,pre_sts;
    uint32 pre_index;
    int i;
    unsigned int *addr,offset,*addr_org;
    unsigned buff_size; 
    tCbuffer *cb_frame_sts = hBufCtrl->cb_frame_sts;
    uint32 *status = &hBufCtrl->bc->frame_status;
    uint32 *index = &hBufCtrl->bc->index;

    pre_sts=0;
    pre_index=0;

    addr_org = addr= cbuffer_get_read_address_ex(cb_frame_sts, &offset);
    buff_size = cbuffer_get_size_in_words(cb_frame_sts);
    for(i=0;i<frm_num;i++){
        sts = *addr;
        addr++;
        if(addr == (unsigned int*)cb_frame_sts->base_addr+buff_size){
            addr = (unsigned int*)cb_frame_sts->base_addr;
        }
        /* frame status */
        if(sts > pre_sts){
            pre_sts = sts;
            pre_index = i;
        }
        if(sts == 3){
            break;
        }
        if((index == 0)&&(sts==0)){
            break;
        }
    }
    /*reset rp*/
    cbuffer_set_read_address_ex(cb_frame_sts, addr_org, offset);
    *status = pre_sts;
    *index = pre_index;
    return CODEC_SUCCESS;
}
#endif // ENABLE_BUFFER_CONTROL

void ldac_decoder_initiliase( LDAC_DEC_PARAMS *pDecParams )
{
    pDecParams->RxBuf.nFrame = 0;
    pDecParams->RxBuf.remaining_FrmNum = 0;
    pDecParams->RxBuf.carried_Octets = 0;
    reset_cbuffers(pDecParams->RxBuf.carried);
    reset_cbuffers(pDecParams->RxBuf.main);
    rxbuf_op_sync_clone(&pDecParams->RxBuf);
    pDecParams->playing = FALSE;
    pDecParams->timer.elapsed_us = 0;
    pDecParams->timer.base_us = 0;
    pDecParams->timer.remained_us=0;
    
    reset_cbuffers(pDecParams->cb_stream);
#ifdef ENABLE_BUFFER_CONTROL
    reset_cbuffers(pDecParams->hBufCtrl.cb_frame_sts);
    
    bc_init(&pDecParams->hBufCtrl);
#endif
}

#ifdef PCM_ADJUST_MORE
int ldac_decoder_get_nFrames_to_decode( LDAC_DEC_PARAMS *pDecParams,void* p_decoder_data, TIME now, int frameNumLimit )
#else
int ldac_decoder_get_nFrames_to_decode( LDAC_DEC_PARAMS *pDecParams, TIME now, int frameNumLimit )
#endif
{
    int nFrmToDec, now_msk, elapsed_us;

#ifdef PCM_ADJUST 
    int first_frm=FALSE;
#endif
    now_msk = now & LDAC_TIMER_MASK;
    nFrmToDec = 0;
#ifdef PCM_ADJUST_MORE
    A2DP_DECODER_PARAMS* p_decoder_data_l = (A2DP_DECODER_PARAMS*)p_decoder_data;
    int initial_space=0;
    int space;
    int close_frm_time=260;//100us
#endif
    if( pDecParams->playing != TRUE )
    {
        if( pDecParams->RxBuf.nFrame < DECODER_NUM_FRAMES_STARTS_DECODE ){// 64: to be changed to the paramer of LDAC_DEC_PARAMS.
            return nFrmToDec; // return 0 because not ready to start playback
        }
        // initialize to start decoding
#ifdef ENABLE_BUFFER_CONTROL
        bc_reset(pDecParams->hBufCtrl.bc, FALSE);
#endif
        pDecParams->playing=TRUE;
        pDecParams->timer.base_us = now_msk;
        // allow 2 frames to play for 1st time
        //elapsed_us is overwrote in the following code.
        pDecParams->timer.elapsed_us = 2*pDecParams->frame_length_us;
#ifdef PCM_ADJUST 
        pDecParams->adjust_frm_count=0;
        first_frm=TRUE;
#endif
    }
    
    elapsed_us = now_msk - pDecParams->timer.base_us;
    if( elapsed_us < 0 ) {
        elapsed_us += LDAC_TIMER_MAX;
    }
#ifdef PCM_ADJUST 
    if(first_frm == TRUE){//first time decode
        // allow 5 frames to play for 1st time
        elapsed_us=5 * pDecParams->frame_length_us;
        first_frm=FALSE;
    }
#endif
    pDecParams->timer.elapsed_us = elapsed_us;//fixed issue
    pDecParams->timer.elapsed_us += pDecParams->timer.remained_us;
    
    nFrmToDec = pDecParams->timer.elapsed_us / pDecParams->frame_length_us;
    if( nFrmToDec > frameNumLimit ) { // cap
        nFrmToDec = frameNumLimit;
    }
#ifdef PCM_ADJUST_MORE //for pcm adjust 
    if((!nFrmToDec) && ((pDecParams->timer.elapsed_us - pDecParams->frame_length_us) < close_frm_time)){ 
        //check pcm output buffer 
        space = cbuffer_calc_amount_space_in_words(p_decoder_data_l->codec.out_left_buffer);
        if ( space == initial_space)
        {
            L2_DBG_MSG2("Left buffer space is empty!, %d word,nFrmToDec=%d",space,nFrmToDec);
        }
    }
    if(nFrmToDec){ 
        //check pcm output buffer 
        space = cbuffer_calc_amount_space_in_words(p_decoder_data_l->codec.out_left_buffer);
        if ( space == initial_space)
        {
            L2_DBG_MSG2("Left buffer space is empty!, %d word,nFrmToDec=%d",space,nFrmToDec);
        }
    }
#endif
#ifdef PCM_ADJUST //for pcm adjust 
    pDecParams->adjust_frm_count+=nFrmToDec;
    if(pDecParams->adjust_frm_count >= pDecParams->adjust_frm_num){
        if(nFrmToDec){    
            nFrmToDec--;
            // pDecParams->adjust_frm_count=nFrmToDec;
            pDecParams->adjust_frm_count = pDecParams->adjust_frm_count - 1 - pDecParams->adjust_frm_num;
            // L2_DBG_MSG2("skip 1frm decode for pcm adjust1=%d nFrame=%d",pDecParams->adjust_frm_count,pDecParams->RxBuf.nFrame);
        }
    }
#endif


    pDecParams->timer.base_us = now_msk;
   
    pDecParams->timer.remained_us = pDecParams->timer.elapsed_us - (pDecParams->frame_length_us * nFrmToDec);
    pDecParams->timer.elapsed_us -= nFrmToDec * pDecParams->frame_length_us;


#ifdef ENABLE_BUFFER_CONTROL
    if(nFrmToDec!=0){
        
        bc_update(pDecParams->hBufCtrl.bc, pDecParams->RxBuf.nFrame, now_msk);
    }
#endif
    return nFrmToDec;
}


int ldac_decoder_decode_frames( LDAC_DEC_PARAMS *pDecParams, unsigned char *p_pcm_out_0, unsigned char *p_pcm_out_1)
{
    LDAC_DEC_RX_BUF *hRxBuf = &pDecParams->RxBuf;
    tCbuffer *cb_stream = pDecParams->cb_stream; // buffer to pass bitstream to decoder
    int used_bytes, wrote_bytes;
    HANDLE_LDAC_BT hData;
    unsigned char *a_stream;
    int n_read_bytes;
    int result, return_mode;
    unsigned int *addr, offset;
    int error_code;
#ifdef ENABLE_BUFFER_CONTROL
    LDAC_BUFF_CTRL_OP *hBufCtrl = &pDecParams->hBufCtrl;
    int data;
    int now_msk;
    TIME now;
#else
    uint32 frame_status;
#endif

    hData = pDecParams->hData;
    /* reset cbuffer read/write pointer */
    reset_cbuffers(cb_stream);

    
#ifdef ENABLE_BUFFER_CONTROL
    switch((int)hBufCtrl->bc->stat){
      case BC_STATE_BURST_SKIP:

        do{
            if ( rxbuf_op_discard_frame_data( hRxBuf, cb_stream ) == CODEC_SUCCESS ){
                hBufCtrl->bc->skip_cnt++;
                /*proceeding frame sts buffer RP*/
                proceed_read_buff(hBufCtrl->cb_frame_sts, &data);
            }else{
                L2_DBG_MSG("error SKIP (BURST) drops a frame");
                break;
            }
        }while(hRxBuf->nFrame > hBufCtrl->bc->frames_upper);
        break;

      case BC_STATE_SKIP:
        if(hBufCtrl->bc->skip_num == 0xFF){
            do{
                int32 drop = 0;
                do{
                    if ( hRxBuf->nFrame < hBufCtrl->bc->frames_target ) {
                        break;
                    }

                    if ( cb_frame_data_chk_sts_iterate(hBufCtrl, hRxBuf, hRxBuf->nFrame) == CODEC_SUCCESS ){
                        /* store frm status */
                        if(hBufCtrl->bc->index==0 && hBufCtrl->bc->frame_status > 0){
                            rxbuf_op_discard_frame_data( hRxBuf, cb_stream);
                            drop += 1;
                            /*proceeding frame sts buffer RP*/
                            proceed_read_buff(hBufCtrl->cb_frame_sts, &data);
                            if(data){
                            }

                        }else if(hBufCtrl->bc->index==0 && hBufCtrl->bc->frame_status == 0){/* frame sts(1,2,3) was not found */
                            hBufCtrl->bc->skip_num = hRxBuf->nFrame;
                            hBufCtrl->bc->skip_flg=0;
                        }else{/* frame sts(1,2,3) was found */
                            hBufCtrl->bc->skip_itr_index = hBufCtrl->bc->index;
                            hBufCtrl->bc->skip_itr_sts = hBufCtrl->bc->frame_status;
                            hBufCtrl->bc->skip_num = hBufCtrl->bc->index;
                            hBufCtrl->bc->skip_flg=1;
                        }
                    }else{
                        hBufCtrl->bc->skip_itr_error=1;
                        L2_DBG_MSG("iterate() error!! ");
                        break;
                    }
                } while (hBufCtrl->bc->index == 0 && hBufCtrl->bc->frame_status==3);
                if (drop) {
                    hBufCtrl->bc->skip_num=0xFF;/* reset iteration flg */
                    hBufCtrl->bc->skip_cnt+=drop;
                }
            }while (0);
        }else{/*frame status iteration have been proceeded before*/
            hBufCtrl->bc->skip_num--;
            if((hBufCtrl->bc->skip_num==0) && (hBufCtrl->bc->skip_flg)){
                rxbuf_op_discard_frame_data( hRxBuf, cb_stream);
                hBufCtrl->bc->skip_num=0xFF;/* reset iteration flg */
                hBufCtrl->bc->skip_flg=0;
                hBufCtrl->bc->skip_cnt++;

                /*proceeding frame sts buffer RP*/
                proceed_read_buff(hBufCtrl->cb_frame_sts, &data);
                if(data){
                }

            }else if(hBufCtrl->bc->skip_num==0){/*searched frame sts=0*/
                hBufCtrl->bc->skip_num=0xFF;/* reset iteration flg */
            }
        }
        break;
      default:
        break;
    }
#endif // ENABLE_BUFFER_CONTROL

#ifdef ENABLE_BUFFER_CONTROL
    // peek 1 frame from hRxBuf to cb_stream
    return_mode = rxbuf_op_peek_frame_data( hRxBuf, cb_stream, &n_read_bytes, &hBufCtrl->bc->frame_status);
#else
    return_mode = rxbuf_op_peek_frame_data( hRxBuf, cb_stream, &n_read_bytes, &frame_status);
#endif
    if (return_mode != CODEC_SUCCESS){ // no data in rx_buf
        if(return_mode == CODEC_ERROR){
            if(search_next_frame_header(hRxBuf,cb_stream,pDecParams->frame_size,pDecParams->sampling_freq,pDecParams->channel_mode)==CODEC_SUCCESS){
#ifdef ENABLE_BUFFER_CONTROL
                rxbuf_op_peek_frame_data( hRxBuf, cb_stream, &n_read_bytes, &hBufCtrl->bc->frame_status);
#else
                rxbuf_op_peek_frame_data( hRxBuf, cb_stream, &n_read_bytes, &frame_status);
#endif
                goto NEXT_STEP;
            }
        }
        /*clear nFrame*/
        hRxBuf->nFrame = 0;
        L2_DBG_MSG("rxbuf_op_peek_frame_data:nFrame=0 !!");

#ifdef ENABLE_BUFFER_CONTROL
        now = time_get_time();
        now_msk = now & LDAC_TIMER_MASK;
        bc_burst(hBufCtrl->bc, 2L * MICROS, now_msk); /* 2sec */ 
#endif
        pDecParams->playing = FALSE; // stop decoding
        return return_mode;
    }

NEXT_STEP:
    
    
    

#ifdef ENABLE_BUFFER_CONTROL
    switch (hBufCtrl->bc->stat ) {
      case BC_STATE_COPY:

        switch (hBufCtrl->bc->num_of_copy) {
          case -1: /* special case; disable continuous copy for status 1 or 2. */
            if ( cb_frame_data_chk_sts_iterate(hBufCtrl, hRxBuf, 1) == CODEC_SUCCESS ){
                if(hBufCtrl->bc->frame_status == 3){
                    hBufCtrl->bc->num_of_copy = 1;
                }else{
                    hBufCtrl->bc->num_of_copy = 0;
                }
            }else{
                L2_DBG_MSG1("iterate() error!! hBufCtrl->bc->num_of_copy=%d",hBufCtrl->bc->num_of_copy);
                hBufCtrl->bc->num_of_copy = 0;
                hBufCtrl->bc->copy_itr_error++;
            }
            break;
          case 0: /* check to see if head frame has non-zero status. */
            hBufCtrl->bc->index=0;
            if ( cb_frame_data_chk_sts_iterate(hBufCtrl, hRxBuf, hRxBuf->nFrame)== CODEC_SUCCESS ){
                /* store frm status */
                if(hBufCtrl->bc->index==0 && hBufCtrl->bc->frame_status > 0){
                    hBufCtrl->bc->num_of_copy = 1;
                }else{
                    hBufCtrl->bc->num_of_copy = 0;
                }
            }else{
                L2_DBG_MSG1("iterate() error!! hBufCtrl->bc->num_of_copy=%d",hBufCtrl->bc->num_of_copy);
                hBufCtrl->bc->num_of_copy = 0;
                hBufCtrl->bc->copy_itr_error++;
            }
            break;
          case 1:
            /* if current frame status is 3 then continue to copy, or break copy */
            if ( cb_frame_data_chk_sts_iterate(hBufCtrl, hRxBuf, 1)== CODEC_SUCCESS ){
                if(hBufCtrl->bc->frame_status == 3){
                    hBufCtrl->bc->num_of_copy = 2;
                }else{
                    hBufCtrl->bc->num_of_copy = -1;
                }
            }else{
                L2_DBG_MSG1("iterate() error!! hBufCtrl->bc->num_of_copy=%d",hBufCtrl->bc->num_of_copy);
                hBufCtrl->bc->num_of_copy = 0;
                hBufCtrl->bc->copy_itr_error++;
            }
            break;
          default:
            /* continous copy limited to 'max_num_of_copy.' */
            if ( cb_frame_data_chk_sts_iterate(hBufCtrl, hRxBuf, 1)== CODEC_SUCCESS ){
                if (++hBufCtrl->bc->num_of_copy >= hBufCtrl->bc->max_num_of_copy ||
                    (hBufCtrl->bc->frame_status != 3) || (hBufCtrl->bc->index!=0)) {
                    hBufCtrl->bc->num_of_copy = 0;
                }
            }else{
                L2_DBG_MSG1("iterate() error!! hBufCtrl->bc->num_of_copy=%d",hBufCtrl->bc->num_of_copy);
                hBufCtrl->bc->num_of_copy = 0;
                hBufCtrl->bc->copy_itr_error++;
            }
            break;
        }

        /* do not drop frame (= copy) if num_of_copy > 0 */
        
        if(hBufCtrl->bc->num_of_copy > 0){
            rxbuf_op_sync_clone(hRxBuf);/* rewind RP */
            hBufCtrl->bc->copy_cnt++;
            break;
        }
      default:
        rxbuf_op_pop_frame_data(hRxBuf, n_read_bytes);/*discard frame*/

        /*proceeding frame sts buffer RP*/
        proceed_read_buff(hBufCtrl->cb_frame_sts, &data);
        if(data){
        }
        break;
    }
#else // ENABLE_BUFFER_CONTROL
    rxbuf_op_pop_frame_data(hRxBuf, n_read_bytes);/*discard frame*/
#endif // ENABLE_BUFFER_CONTROL

    addr = cbuffer_get_read_address_ex(cb_stream, &offset);
    a_stream = (unsigned char*)(addr + offset);


    result = ldacBT_decode_S32( hData, a_stream, p_pcm_out_0, p_pcm_out_1,n_read_bytes, &used_bytes, &wrote_bytes );
    if (result) {
        error_code = ldacBT_get_error_code(hData);
        L2_DBG_MSG1("ldacBT_get_error_code=0x%x",error_code);
        return_mode = CODEC_ERROR; //TODO: tentative value. must replace to correct one.
        return return_mode;
    }
    /*decode success*/
    return_mode = CODEC_SUCCESS;


    if((pDecParams->channel_mode==LDACBT_CHANNEL_MODE_STEREO)||(pDecParams->channel_mode==LDACBT_CHANNEL_MODE_DUAL_CHANNEL)){
        pDecParams->num_output_samples = wrote_bytes/2/4;/*32bit pcm*/
    }else{//MONO
        pDecParams->num_output_samples = wrote_bytes/4;/*32bit pcm*/
    }
        
    pDecParams->frame_size = used_bytes;
    return return_mode;
}
    
    
